#define __AccelerateCalculus_C
#include "AccelerateCalculus.h"
#include "mpu6050.h"
#include <math.h>
int64_t totalx,totaly,totalz;
/**
 * @brief           初始化数据区                                      
 *  @details        立刻采样数据区长度次数的数据，填满数据区，并且将下标值归0（貌似不归0也没有影响）
 * 
 * 
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-11-17
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-11-17
 * @version         1.0.0
 */
void InitAccelerate(){
    totalx=totaly=totalz=0;
    for(BufferPoint=0;BufferPoint<AverageSize;BufferPoint++){
        RISCVReadMPU(&AverageBuffer[BufferPoint],ReadType);
        /*totalx+=AverageBuffer[BufferPoint].x;
        totaly+=AverageBuffer[BufferPoint].y;
        totalz+=AverageBuffer[BufferPoint].z;*/
        totalx+=AverageBuffer[BufferPoint].x;
        totaly+=AverageBuffer[BufferPoint].y;
        totalz+=AverageBuffer[BufferPoint].z;
    }
    BufferPoint=0;
}

/**
 * @brief           获得滤波后的当前数据                                      
 *  @details        对当前所有的数据用滤波算法进行调整，然后输出滤波后的数据
 * 
 * @param           参数名称:【data】         数据类型:             参数说明
 * 
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-11-15
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-11-15
 * @version         1.0.0
 */
void GetAccelerate(Vector3_16* data){/*
    int end=BufferPoint+AverageBuffer;
    Vector3_16 Average;
    Average.x=Average.y=Average.z=0;
    for(int i=0;i<AverageSize;i++){
        Average.x+=AverageBuffer[i].x;
        Average.y+=AverageBuffer[i].y;
        Average.z+=AverageBuffer[i].z;
        //Average.x+=AverageBuffer[i%AverageSize].x;
        //Average.y+=AverageBuffer[i%AverageSize].y;
        //Average.z+=AverageBuffer[i%AverageSize].z;
    }
    data[0].x=Average.x/AverageSize;
    data[0].y=Average.y/AverageSize;
    data[0].z=Average.z/AverageSize;*/
    data[0].x=(totalx);
    data[0].y=(totaly);
    data[0].z=(totalz);
}
/**
 * @brief           加载新数据                                      
 *  @details        从MPU6050中读取一个最新的数据，覆盖掉缓冲区中最旧的数据
 * 
 * 
 * *//*
 * 创建者:             Winyunq
 * 创建日期:            2022-11-15
 * 
 *      《初始化》
 * 修订内容:            创建函数
 * @author          Winyunq进行完善
 * @date            2022-11-15
 * @version         1.0.0
 */
void LoadAccelerate(){
        totalx-=AverageBuffer[BufferPoint].x;
        totaly-=AverageBuffer[BufferPoint].y;
        totalz-=AverageBuffer[BufferPoint].z;
    RISCVReadMPU(&AverageBuffer[BufferPoint],ReadType);
    BufferPoint=(BufferPoint+1)%AverageSize;
        totalx+=AverageBuffer[BufferPoint].x;
        totaly+=AverageBuffer[BufferPoint].y;
        totalz+=AverageBuffer[BufferPoint].z;
}

#ifndef KALMAN_H_
#define KALMAN_H_

typedef struct {
    double Q_angle;
    double Q_bias;
    double R_measure;
    double angle;
    double bias;
    double P[2][2];
} Kalman_t;



#endif /* KALMAN_H_ */

#define RAD_TO_DEG 57.295779513082320876798154814105
//#define double float
typedef struct{
    double Accel_X;
    double Accel_Y;
    double Accel_Z;
    double Gyro_X;
    double Gyro_Y;
    double Gyro_Z;
    double Angle_X;
    double Angle_Y;
    double Angle_Z;
    double Temp;
    double KalmanAngleX;
    double KalmanAngleY;
}IMU_Parameter;


IMU_Parameter IMU_Data;

uint32_t timer;

Kalman_t KalmanX = {
    .Q_angle = 0.001f,
    .Q_bias = 0.003f,
    .R_measure = 0.03f};

Kalman_t KalmanY = {
    .Q_angle = 0.001f,
    .Q_bias = 0.003f,
    .R_measure = 0.03f,
};

double Kalman_getAngle(Kalman_t *Kalman, double newAngle, double newRate, uint32 dt)
{
    double rate = newRate - Kalman->bias;
    Kalman->angle += dt * rate;

    Kalman->P[0][0] += dt * (dt * Kalman->P[1][1] - Kalman->P[0][1] - Kalman->P[1][0] + Kalman->Q_angle);
    Kalman->P[0][1] -= dt * Kalman->P[1][1];
    Kalman->P[1][0] -= dt * Kalman->P[1][1];
    Kalman->P[1][1] += Kalman->Q_bias * dt;

    double S = Kalman->P[0][0] + Kalman->R_measure;
    double K[2];
    K[0] = Kalman->P[0][0] / S;
    K[1] = Kalman->P[1][0] / S;

    double y = newAngle - Kalman->angle;
    Kalman->angle += K[0] * y;
    Kalman->bias += K[1] * y;

    double P00_temp = Kalman->P[0][0];
    double P01_temp = Kalman->P[0][1];

    Kalman->P[0][0] -= K[0] * P00_temp;
    Kalman->P[0][1] -= K[0] * P01_temp;
    Kalman->P[1][0] -= K[1] * P00_temp;
    Kalman->P[1][1] -= K[1] * P01_temp;

    return Kalman->angle;
};

void MPU6050_Read_All(IMU_Parameter *IMU_Data,int32_t dt)
{
    double roll;
    double roll_sqrt = sqrt(IMU_Data->Accel_X * IMU_Data->Accel_X + IMU_Data->Accel_Z * IMU_Data->Accel_Z);
    if (roll_sqrt != 0.0)
    {
        roll = atan(IMU_Data->Accel_Y / roll_sqrt) * RAD_TO_DEG;
    }
    else
    {
        roll = 0.0;
    }
    double pitch = atan2(-IMU_Data->Accel_X, IMU_Data->Accel_Z) * RAD_TO_DEG;
    if ((pitch < -90 && IMU_Data->KalmanAngleY > 90) || (pitch > 90 && IMU_Data->KalmanAngleY < -90))
    {
        KalmanY.angle = pitch;
        IMU_Data->KalmanAngleY = pitch;
    }
    else
    {
        IMU_Data->KalmanAngleY = Kalman_getAngle(&KalmanY, pitch, IMU_Data->Gyro_Y, dt);
    }
    if (fabs(IMU_Data->KalmanAngleY) > 90)
        IMU_Data->Gyro_X = -IMU_Data->Gyro_X;
    IMU_Data->KalmanAngleX = Kalman_getAngle(&KalmanX, roll, IMU_Data->Gyro_Y, dt);
    printf("\nKalman:X=%lf,Y=%lf",IMU_Data->KalmanAngleX,IMU_Data->KalmanAngleY);
}
void Kalman(int32_t time){
    printf("\ndt=%d\n",time);
    Vector3_16 acc,gyro;

    RISCVReadMPU(&acc,MPU6050Accelerate);
    RISCVReadMPU(&gyro,MPU6050Angle);
    IMU_Data.Accel_X=acc.x;
    IMU_Data.Accel_Y=acc.y;
    IMU_Data.Accel_Z=acc.z;
    printf("\nacce: X=%lf,Y=%lf,Z=%.7lf\n",IMU_Data.Accel_X,IMU_Data.Accel_Y,IMU_Data.Accel_Z);
    IMU_Data.Gyro_X=gyro.x;
    IMU_Data.Gyro_Y=gyro.y;
    IMU_Data.Gyro_Z=gyro.z;
    printf("\ngyro: X=%lf,Y=%lf,Z=%lf\n",IMU_Data.Gyro_X,IMU_Data.Gyro_Y,IMU_Data.Gyro_Z);
    MPU6050_Read_All(&IMU_Data,time);
}